#ifndef BEZIERCUBCURVE_H_
#include "BezierCubCurve.h"
#endif

using namespace std;
using std::vector;

BezierCubCurve :: BezierCubCurve(vector<float>* Points, float vectorPerp[3]){
 controlPoints = new vector<float>();
 controlPoints->insert(controlPoints->end(), Points->begin(), Points->end());

 curve = new vector<float>();
 calculateCurve();

 normals = new vector<float>();
 calculateNormals(vectorPerp);
}

BezierCubCurve :: ~BezierCubCurve(){
 delete(controlPoints);
 delete(curve);
 delete (normals);
}

void BezierCubCurve :: calculateCurve(){
 curve = new  vector<float>();
 for (float p=0; p < controlPoints->size()-9;p+=9){

 	float p0[3] = {(*controlPoints)[p],(*controlPoints)[p+1],(*controlPoints)[p+2]};
 	float p1[3] = {(*controlPoints)[p+3],(*controlPoints)[p+4],(*controlPoints)[p+5]};
 	float p2[3] = {(*controlPoints)[p+6],(*controlPoints)[p+7],(*controlPoints)[p+8]};
	float p3[3] = {(*controlPoints)[p+9],(*controlPoints)[p+10],(*controlPoints)[p+11]};

 	for (float t=0; t<=1;t+=(float)(1/(float)TAMBCUB)){
 
		float b0 = (1-t)*(1-t)*(1-t);
		float b1 = 3*(1-t)*(1-t)*t;
		float b2 = 3*(1-t)*t*t;
		float b3 = t*t*t;

		float x = p0[0]*b0 + p1[0]*b1 + p2[0]*b2 + p3[0]*b3;
		float y = p0[1]*b0 + p1[1]*b1 + p2[1]*b2 + p3[1]*b3;	
		float z = p0[2]*b0 + p1[2]*b1 + p2[2]*b2 + p3[2]*b3;

		curve->push_back(x);
		curve->push_back(y);
		curve->push_back(z);
 	}
 }
}

void BezierCubCurve :: calculateNormals(float vectorPerp[3]){
 float h = 0.1; 
 size_t t=0;
 for (t=0; t<curve->size()-5 ;t+=3){				// dx                      dy              dz
								// vectorPerp[0] vectorPerp[1]  vectorPerp[2]
	float dx = ((*curve)[t+3] - (*curve)[t])/h ;		// x' = dy*vectorPerp[2]-dz*vectorPerp[1]
	float dy = ((*curve)[t+4] - (*curve)[t+1])/h;		// y' = dz*vectorPerp[0]-dx*vectorPerp[2]
	float dz = ((*curve)[t+5] - (*curve)[t+2])/h;		// z' = dx*vectorPerp[1]-dy*vectorPerp[0]

	float x = dy*vectorPerp[2]-dz*vectorPerp[1];
	float y = dz*vectorPerp[0]-dx*vectorPerp[2];
	float z = dx*vectorPerp[1]-dy*vectorPerp[0];

	float mod = sqrt(x*x+y*y+z*z);

	normals->push_back(x/mod); 				
	normals->push_back(y/mod);				
 	normals->push_back(z/mod);
 }

 float dx = ((*curve)[t]   - (*curve)[t-3])/h;		
 float dy = ((*curve)[t+1] - (*curve)[t-2])/h;

 float mod = sqrt(dx*dx+dy*dy);

 normals->push_back(dy/mod); 				
 normals->push_back((-1)*dx/mod);				
 normals->push_back(0);	
}

size_t BezierCubCurve :: getSize(){
	return curve->size();
}

// hace un append de toda la curva en el vector
void BezierCubCurve :: getCurve(vector<float>* targetCurve){
 targetCurve->insert(targetCurve->end(), curve->begin(), curve->end());
}

void BezierCubCurve :: getNormals(vector<float>* targetNormals){
 targetNormals->insert(targetNormals->end(), normals->begin(), normals->end());
}								

void BezierCubCurve :: drawNormals(){
 for (float t=0; t< normals->size() ;t+=9){	
 	glBegin(GL_LINE_STRIP);
		glVertex3f((*curve)[t],(*curve)[t+1],(*curve)[t+2]);
		glVertex3f((*normals)[t]+(*curve)[t],(*normals)[t+1]+(*curve)[t+1],(*normals)[t+2]+(*curve)[t+2]);
	glEnd();	
 }
}

void BezierCubCurve :: drawCurve(glm::mat4 mat){
 glBegin(GL_LINE_STRIP);
 for (float t=0; t< curve->size() ;t+=3){	
		glm::vec4 Position = glm::vec4((*curve)[t],(*curve)[t+1],(*curve)[t+2],1.0f) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);
 }
 glEnd();
}
void BezierCubCurve :: drawControlPoints(){
// glPointSize(3.0);
// glBegin(GL_POINTS);
 glBegin(GL_LINE_STRIP);
 for (float t=0; t< controlPoints->size() ;t+=3)	
		glVertex3f((*controlPoints)[t],(*controlPoints)[t+1],(*controlPoints)[t+2]);
 glEnd();
}
